"""
作为状态显示器的单元处理器

date: 2025/3/19
author: SiHeng Tang
file: status.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging
from copy import deepcopy

from async_sche import TaskWrapper, BaseTaskGen
from mengji import Vehicle
from mod_loader import ConfigHandler
from task_element import DataEndpoint

logger = logging.getLogger(__name__)
handler = ConfigHandler(__name__)

DEFAULT_CONFIG = {
    "label": "status"
}

STATUS_COMMAND_USAGE = ("Print plane flight status command interface\n"
                        "Usage:\n"
                        "    status [sub_command] [args]\n"
                        "Available sub command:\n"
                        "    console_print_off, console_print_on")

STATUS_STRING = ("PLANE STATUS DATA: \n"
                 "GPS Main info lat: %(lat)f Deg lon: %(lon)f Deg\n"
                 "abs_alt: %(alt)d Meters  alt: %(relative_alt)d Meters\n"
                 "GPS Speed info: vx: %(vx)d vy: %(vy)d vz: %(vz)d\n"
                 "Heading Direction: %(hdg)s Deg\n"
                 "Flight mode %(custom_mode_string)s\n"
                 "Mission status %(seq)d / %(total)d\n")


class DisplayStatus(BaseTaskGen):
    def __init__(self, config: dict):
        super().__init__(priority=TaskWrapper.IDLE_PRIORITY + 10)

        self.recv_gps = DataEndpoint()
        self.recv_attitude = DataEndpoint()
        self.recv_flight_mode = DataEndpoint()
        self.recv_mc = DataEndpoint()

        self._enabled_console_print = config["default_console_print"]
        self._interval = config["interval"]

    def _on_exit(self):
        self.recv_gps = None
        self.recv_attitude = None
        self.recv_flight_mode = None
        self.recv_mc = None

    def topic_GLOBAL_POSITION(self, rpc_params: str):
        """
        GPS 位置估算话题
        :param rpc_params: 话题参数
        :return: 如果当前 GPS 估算数据可用，返回 GPS 位置估算数据包，不可用返回 0
        """
        if rpc_params == "subscribe":
            return 0

        if rpc_params == "unsubscribe":
            return 0

        if rpc_params == "mailbox_update":
            if res := self.recv_gps.get_dp().get_a("payload"):
                return res
            else:
                return 0

        return -1

    def topic_ATTITUDE(self, rpc_params: str):
        """
        飞行器姿态话题
        :param rpc_params: 话题参数
        :return: 如果当前飞行器姿态数据可用，返回飞行器姿态数据包，不可用返回 0
        """
        if rpc_params == "subscribe":
            return 0

        if rpc_params == "unsubscribe":
            return 0

        if rpc_params == "mailbox_update":
            if res := self.recv_attitude.get_dp().get_a("payload"):
                return res
            else:
                return 0

        return -1

    def topic_FM(self, rpc_params: str):
        """
        飞行模式话题
        :param rpc_params: 话题参数
        :return: 如果当前飞行器模式数据可用，返回飞行器模式数据包，不可用返回 0
        """
        if rpc_params == "subscribe":
            return 0

        if rpc_params == "unsubscribe":
            return 0

        if rpc_params == "mailbox_update":
            if res := self.recv_flight_mode.get_dp().get_a("payload"):
                return res
            else:
                return 0

        return -1


@handler.add_handler("unit_proc:status")
def load_config(veh: Vehicle, cfg_section: dict):
    logger.debug(f"status load config {cfg_section}")

    cfg = deepcopy(DEFAULT_CONFIG)
    cfg.update(cfg_section)

    veh.reg_part(cfg["label"], node := DisplayStatus(cfg), is_sche=True)
    veh.get_a("api").add_topic("status/fm", node.topic_FM)
    veh.get_a("api").add_topic("status/gps", node.topic_GLOBAL_POSITION)
    veh.get_a("api").add_topic("status/attitude", node.topic_ATTITUDE)
